// --------------------------------------------------------------------------------------------------------------------
// <copyright file="SlamRecordingTransformerAgent.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Test.Robotics.Navigation
{
    using System;
    using System.Collections.Generic;
    using System.Diagnostics;
    using System.IO;
    using System.Runtime.Serialization;

    using Microsoft.Robotics.Hardware.DAC;
    using Microsoft.Robotics.Hardware.Runtime.DAC;
    using Microsoft.Robotics.Interaction.Runtime.Input;
    using Microsoft.Robotics.Numerics;
    using Microsoft.Robotics.Runtime;
    using Microsoft.Robotics.Vision.Cameras;
    using Microsoft.Robotics.Vision.Runtime.Cameras;

    /// <summary>
    /// SlamRecordingTransformerAgent allows you to convert an existing recording with turret rotation and wheel motion
    /// into a recording with only turret rotation. 
    /// SlamRecordingTransformerAgent can also be used in live mode without any changes to play an existing recording with only turret motion.    
    /// It accepts a parameter which determines whether the published odometry is from the original recording or synthesized from the
    /// ground truth file to be "perfect" odometry.
    /// It accepts a parameter to control the threshold for turret pan change before publishing a new message.
    /// </summary>
    /// <typeparam name="TDriveFeedbackState">The drive feedback state type used for motion update</typeparam>
    [DataContract]
    public class SlamRecordingTransformerAgent<TDriveFeedbackState> :
        ConsumerProducerAgent<DepthCameraAgentMessage<KinectDepthCameraParameters>, DepthCameraAgentMessage<KinectDepthCameraParameters>>,
        ISubscriptionReceiver<GamePadAgentMessage>,
        ISubscriptionReceiver<AccelerometerMessage>,
        ISubscriptionReceiver<CompassMessage>,
        ISubscriptionReceiver<GyroscopeMessage>,
        ISubscriptionReceiver<SonarMessage>,
        ISubscriptionReceiver<IRProximityMessage>,
        ISubscriptionReceiver<DifferentialDriveMessage>,
        ISubscriptionReceiver<BatteryMessage>,
        ISubscriptionReceiver<JointMessage>,
        ISubscriptionReceiver<SimpleAgentMessage<TDriveFeedbackState>>,
        IMessagePublisher<DepthCameraAgentMessage<KinectDepthCameraParameters>>,
        IMessagePublisher<AccelerometerMessage>,
        IMessagePublisher<CompassMessage>,
        IMessagePublisher<GyroscopeMessage>,
        IMessagePublisher<SonarMessage>,
        IMessagePublisher<IRProximityMessage>,
        IMessagePublisher<DifferentialDriveMessage>,
        IMessagePublisher<SimpleAgentMessage<TDriveFeedbackState>>,
        IMessagePublisher<BatteryMessage>,
        IMessagePublisher<JointMessage>,
        IMessagePublisher<GamePadAgentMessage> where TDriveFeedbackState : IDriveState
    {
        /// <summary>
        /// Default turret pan threshold which determines the change in turret angle required before publishing a new message.
        /// </summary>
        private const double DefaultTurretPanThreshold = MathConstants.Degrees2Radians * 5;

        /// <summary>
        /// The threshold for turret pan angle change which triggers publishing of a new message.
        /// </summary>
        [DataMember]
        private readonly double turretPanThreshold;

        /// <summary>
        /// Producer name.
        /// </summary>
        [DataMember]
        private string gamepadProducer;

        /// <summary>
        /// Agent publishing the raw messages.
        /// </summary>
        private IAgentHost gamePad;

        /// <summary>
        /// Forwarder publishing the processed messages.
        /// </summary>
        private IForwarder<GamePadAgentMessage> gamePadPublisher;

        /// <summary>
        /// Producer name.
        /// </summary>
        [DataMember]
        private string accelerometerProducer;

        /// <summary>
        /// Agent publishing the raw messages.
        /// </summary>
        private IAgentHost accelerometer;

        /// <summary>
        /// Forwarder publishing the processed messages.
        /// </summary>
        private IForwarder<AccelerometerMessage> accelerometerPublisher;

        /// <summary>
        /// Producer name.
        /// </summary>
        [DataMember]
        private string compassProducer;

        /// <summary>
        /// Agent publishing the raw messages.
        /// </summary>
        private IAgentHost compass;

        /// <summary>
        /// Forwarder publishing the processed messages.
        /// </summary>
        private IForwarder<CompassMessage> compassPublisher;

        /// <summary>
        /// Producer name.
        /// </summary>
        [DataMember]
        private string gyroscopeProducer;

        /// <summary>
        /// Agent publishing the raw messages.
        /// </summary>
        private IAgentHost gyroscope;

        /// <summary>
        /// Forwarder publishing the processed messages.
        /// </summary>
        private IForwarder<GyroscopeMessage> gyroscopePublisher;

        /// <summary>
        /// Producer name.
        /// </summary>
        [DataMember]
        private string sonarProducer;

        /// <summary>
        /// Agent publishing the raw messages.
        /// </summary>
        private IAgentHost sonar;

        /// <summary>
        /// Forwarder publishing the processed messages.
        /// </summary>
        private IForwarder<SonarMessage> sonarPublisher;

        /// <summary>
        /// Producer name.
        /// </summary>
        [DataMember]
        private string proximityProducer;

        /// <summary>
        /// Agent publishing the raw messages.
        /// </summary>
        private IAgentHost proximity;

        /// <summary>
        /// Forwarder publishing the processed messages.
        /// </summary>
        private IForwarder<IRProximityMessage> proximityPublisher;

        /// <summary>
        /// Producer name.
        /// </summary>
        [DataMember]
        private string differentialDriveProducer;

        /// <summary>
        /// Agent publishing the raw messages.
        /// </summary>
        private IAgentHost differentialDrive;

        /// <summary>
        /// Forwarder publishing the processed messages.
        /// </summary>
        private IForwarder<DifferentialDriveMessage> differentialDrivePublisher;

        /// <summary>
        /// Producer name.
        /// </summary>
        [DataMember]
        private string driveFeedbackProducer;

        /// <summary>
        /// Agent publishing the raw messages.
        /// </summary>
        private IAgentHost driveFeedback;

        /// <summary>
        /// Forwarder publishing the processed messages.
        /// </summary>
        private IForwarder<SimpleAgentMessage<TDriveFeedbackState>> driveFeedbackPublisher;

        /// <summary>
        /// Producer name.
        /// </summary>
        [DataMember]
        private string batteryProducer;

        /// <summary>
        /// Agent publishing the raw messages.
        /// </summary>
        private IAgentHost battery;

        /// <summary>
        /// Forwarder publishing the processed messages.
        /// </summary>
        private IForwarder<BatteryMessage> batteryPublisher;

        /// <summary>
        /// Producer name.
        /// </summary>
        [DataMember]
        private string jointProducer;

        /// <summary>
        /// Agent publishing the raw messages.
        /// </summary>
        private IAgentHost joint;

        /// <summary>
        /// Forwarder publishing the processed messages.
        /// </summary>
        private IForwarder<JointMessage> jointPublisher;

        /// <summary>
        /// The previous frame's game buttons
        /// </summary>
        private GamePadAgentMessage.GamePadAgentButtons lastButtons;

        /// <summary>
        /// Ground truth absolute file.
        /// </summary>
        [DataMember]
        private string groundTruthAbsoluteFile;

        /// <summary>
        /// Previously published angle.
        /// </summary>
        private double previousPublishedAngle;

        /// <summary>
        /// Timestamp for next message
        /// </summary>
        private long originatingTime;

        /// <summary>
        /// The angle to consider as zeroPanAngle
        /// </summary>
        private double? zeroPanAngle;

        /// <summary>
        /// Ground truth absolute poses.
        /// </summary>
        private Queue<Pose2D> groundTruthAbsolutePoses;

        /// <summary>
        /// Current ground truth pose.
        /// </summary>
        private Pose2D? currentGroundTruthPose;

        /// <summary>
        /// Current ground truth pose.
        /// </summary>
        private Pose2D? previousGroundTruthPose;

        /// <summary>
        /// Flags to keep track of the last published timestamps.
        /// </summary>
        private Dictionary<Type, long> publishedTimestamps;

        /// <summary>
        /// Initializes a new instance of the <see cref="SlamRecordingTransformerAgent{TDriveFeedbackState}"/> class.
        /// </summary>
        /// <param name="name">Name of the agent.</param>
        /// <param name="groundTruthAbsoluteFile">Ground truth absolute file</param>
        /// <param name="kinectProducer">Kinect message producer</param>
        /// <param name="gamepadProducer">Gamepad message producer</param>
        /// <param name="accelerometerProducer">Accelerometer message producer</param>
        /// <param name="compassProducer">Compass message producer</param>
        /// <param name="gyroscopeProducer">Gyroscope message producer</param>
        /// <param name="sonarProducer">Sonar message producer</param>
        /// <param name="proximityProducer">Proximity message producer</param>
        /// <param name="differentialDriveProducer">Drive message producer</param>
        /// <param name="batteryProducer">Battery message producer</param>                
        /// <param name="jointProducer">Joint message producer</param>
        /// <param name="driveFeedbackProducer">Drive feedback producer</param>
        /// <param name="useOriginalOdometry">Indicates whether to use the original odometry or the perfect odometry from ground truth</param>
        /// <param name="turretPanThreshold">The threshold for turret pan angle change which triggers publishing of a new message</param>
        public SlamRecordingTransformerAgent(
            string name,
            string groundTruthAbsoluteFile,
            IProducer<DepthCameraAgentMessage<KinectDepthCameraParameters>> kinectProducer,
            IProducer<GamePadAgentMessage> gamepadProducer,
            IProducer<AccelerometerMessage> accelerometerProducer,
            IProducer<CompassMessage> compassProducer,
            IProducer<GyroscopeMessage> gyroscopeProducer,
            IProducer<SonarMessage> sonarProducer,
            IProducer<IRProximityMessage> proximityProducer,
            IProducer<DifferentialDriveMessage> differentialDriveProducer,
            IProducer<BatteryMessage> batteryProducer,
            IProducer<JointMessage> jointProducer,
            IProducer<SimpleAgentMessage<TDriveFeedbackState>> driveFeedbackProducer,
            bool useOriginalOdometry = true,
            double turretPanThreshold = DefaultTurretPanThreshold)
            : base(name, kinectProducer.Name)
        {
            this.groundTruthAbsoluteFile = groundTruthAbsoluteFile;
            this.gamepadProducer = gamepadProducer.Name;
            this.accelerometerProducer = accelerometerProducer.Name;
            this.compassProducer = compassProducer.Name;
            this.gyroscopeProducer = gyroscopeProducer.Name;
            this.sonarProducer = sonarProducer.Name;
            this.proximityProducer = proximityProducer.Name;
            this.differentialDriveProducer = differentialDriveProducer.Name;
            this.batteryProducer = batteryProducer.Name;
            this.jointProducer = jointProducer.Name;
            this.driveFeedbackProducer = driveFeedbackProducer.Name;
            this.turretPanThreshold = turretPanThreshold;
        }

        /// <summary>
        /// Finds and subscribes to the publishing agent.
        /// </summary>
        /// <param name="locator">A locator to use.</param>
        public override void Initialize(AgentLocator locator)
        {
            base.Initialize(locator);
            this.gamePad = locator.GetAgent(this.gamepadProducer);
            this.accelerometer = locator.GetAgent(this.accelerometerProducer);
            this.compass = locator.GetAgent(this.compassProducer);
            this.gyroscope = locator.GetAgent(this.gyroscopeProducer);
            this.sonar = locator.GetAgent(this.sonarProducer);
            this.proximity = locator.GetAgent(this.proximityProducer);
            this.differentialDrive = locator.GetAgent(this.differentialDriveProducer);
            this.battery = locator.GetAgent(this.batteryProducer);
            this.joint = locator.GetAgent(this.jointProducer);
            this.driveFeedback = locator.GetAgent(this.driveFeedbackProducer);
            this.gamePad.Subscribe<GamePadAgentMessage>(this);
            this.accelerometer.Subscribe<AccelerometerMessage>(this);
            this.compass.Subscribe<CompassMessage>(this);
            this.gyroscope.Subscribe<GyroscopeMessage>(this);
            this.sonar.Subscribe<SonarMessage>(this);
            this.proximity.Subscribe<IRProximityMessage>(this);
            this.differentialDrive.Subscribe<DifferentialDriveMessage>(this);
            this.joint.Subscribe<JointMessage>(this);
            this.driveFeedback.Subscribe<SimpleAgentMessage<TDriveFeedbackState>>(this);
            this.previousPublishedAngle = double.NaN;
            this.originatingTime = 0;
            this.groundTruthAbsolutePoses = new Queue<Pose2D>();
            this.publishedTimestamps = new Dictionary<Type, long>();
        }

        /// <summary>
        /// Initializer that provides a way for us to broadcast messages.
        /// </summary>
        /// <param name="forwarder">A forwarder that can be used to broadcast our state.</param>
        public void InitializePublisher(IForwarder<AccelerometerMessage> forwarder)
        {
            this.accelerometerPublisher = forwarder;
        }

        /// <summary>
        /// Initializer that provides a way for us to broadcast messages.
        /// </summary>
        /// <param name="forwarder">A forwarder that can be used to broadcast our state.</param>
        public void InitializePublisher(IForwarder<BatteryMessage> forwarder)
        {
            this.batteryPublisher = forwarder;
        }

        /// <summary>
        /// Initializer that provides a way for us to broadcast messages.
        /// </summary>
        /// <param name="forwarder">A forwarder that can be used to broadcast our state.</param>
        public void InitializePublisher(IForwarder<CompassMessage> forwarder)
        {
            this.compassPublisher = forwarder;
        }

        /// <summary>
        /// Initializer that provides a way for us to broadcast messages.
        /// </summary>
        /// <param name="forwarder">A forwarder that can be used to broadcast our state.</param>
        public void InitializePublisher(IForwarder<DifferentialDriveMessage> forwarder)
        {
            this.differentialDrivePublisher = forwarder;
        }

        /// <summary>
        /// Initializer that provides a way for us to broadcast messages.
        /// </summary>
        /// <param name="forwarder">A forwarder that can be used to broadcast our state.</param>
        public void InitializePublisher(IForwarder<SimpleAgentMessage<TDriveFeedbackState>> forwarder)
        {
            this.driveFeedbackPublisher = forwarder;
        }

        /// <summary>
        /// Initializer that provides a way for us to broadcast messages.
        /// </summary>
        /// <param name="forwarder">A forwarder that can be used to broadcast our state.</param>
        public void InitializePublisher(IForwarder<GyroscopeMessage> forwarder)
        {
            this.gyroscopePublisher = forwarder;
        }

        /// <summary>
        /// Initializer that provides a way for us to broadcast messages.
        /// </summary>
        /// <param name="forwarder">A forwarder that can be used to broadcast our state.</param>
        public void InitializePublisher(IForwarder<IRProximityMessage> forwarder)
        {
            this.proximityPublisher = forwarder;
        }

        /// <summary>
        /// Initializer that provides a way for us to broadcast messages.
        /// </summary>
        /// <param name="forwarder">A forwarder that can be used to broadcast our state.</param>
        public void InitializePublisher(IForwarder<JointMessage> forwarder)
        {
            this.jointPublisher = forwarder;
        }

        /// <summary>
        /// Initializer that provides a way for us to broadcast messages.
        /// </summary>
        /// <param name="forwarder">A forwarder that can be used to broadcast our state.</param>
        public void InitializePublisher(IForwarder<SonarMessage> forwarder)
        {
            this.sonarPublisher = forwarder;
        }

        /// <summary>
        /// Initializer that provides a way for us to broadcast messages.
        /// </summary>
        /// <param name="forwarder">A forwarder that can be used to broadcast our state.</param>
        public void InitializePublisher(IForwarder<GamePadAgentMessage> forwarder)
        {
            this.gamePadPublisher = forwarder;
        }

        /// <summary>
        /// Called when the agent is about to start running
        /// </summary>
        public override void OnActivated()
        {
            this.gamePad.Activate();
            this.accelerometer.Activate();
            this.compass.Activate();
            this.gyroscope.Activate();
            this.sonar.Activate();
            this.proximity.Activate();
            this.battery.Activate();
            this.joint.Activate();
            this.driveFeedback.Activate();
            if (!string.IsNullOrEmpty(this.groundTruthAbsoluteFile))
            {
                using (StreamReader fileReader = new StreamReader(this.groundTruthAbsoluteFile))
                {
                    Pose2D absolute = new Pose2D();
                    this.groundTruthAbsolutePoses.Enqueue(absolute);
                    int counter = 0;
                    while (!fileReader.EndOfStream)
                    {
                        string line = fileReader.ReadLine();
                        if (string.IsNullOrEmpty(line) || line[0] == '#')
                        {
                            continue;
                        }

                        string[] data = line.Split(new[] { ' ', '\t' }, StringSplitOptions.RemoveEmptyEntries);
                        double heading = double.Parse(data[7]);
                        double absoluteHeading = Pose2D.ClampAngle(heading + absolute.Heading);
                        double deltaX = double.Parse(data[2]);
                        double deltaY = double.Parse(data[3]);
                        double r = Math.Sqrt(deltaX * deltaX + deltaY * deltaY);

                        double rotatedX = r * Math.Cos(absoluteHeading);
                        double rotatedY = r * Math.Sin(absoluteHeading);
                        Pose2D poseDelta = new Pose2D(rotatedX, rotatedY, heading);
                        absolute += poseDelta;
                        this.groundTruthAbsolutePoses.Enqueue(absolute);
                        counter++;
                    }
                }
            }

            base.OnActivated();
        }

        /// <summary>
        /// Called when the agent is about to end
        /// </summary>
        public override void OnDeactivated()
        {
            this.gamePad.Deactivate();
            this.accelerometer.Deactivate();
            this.compass.Deactivate();
            this.gyroscope.Deactivate();
            this.sonar.Deactivate();
            this.proximity.Deactivate();
            this.battery.Deactivate();
            this.joint.Deactivate();
            this.driveFeedback.Deactivate();

            this.groundTruthAbsolutePoses.Clear();
            this.currentGroundTruthPose = null;

            base.OnDeactivated();
        }

        /// <summary>
        /// Receive, process and republish a message
        /// </summary>
        /// <param name="message">Message to process</param>
        public override void Receive(DepthCameraAgentMessage<KinectDepthCameraParameters> message)
        {
            this.Publish<DepthCameraAgentMessage<KinectDepthCameraParameters>>(this.Publisher, message);
        }

        /// <summary>
        /// Receive, process and republish a message
        /// </summary>
        /// <param name="message">Message to process</param>
        public void Receive(GamePadAgentMessage message)
        {
            if (this.lastButtons != null)
            {
                if (message.Buttons.LeftShoulder && (!this.lastButtons.LeftShoulder))
                {
                    this.previousGroundTruthPose = this.currentGroundTruthPose;
                    this.currentGroundTruthPose = this.groundTruthAbsolutePoses.Dequeue();
                }
            }

            this.lastButtons = message.Buttons;

            this.Publish<GamePadAgentMessage>(this.gamePadPublisher, message);
        }

        /// <summary>
        /// Receive, process and republish a message
        /// </summary>
        /// <param name="message">Message to process</param>
        public void Receive(SonarMessage message)
        {
            this.Publish<SonarMessage>(this.sonarPublisher, message);
        }

        /// <summary>
        /// Receive, process and republish a message
        /// </summary>
        /// <param name="message">Message to process</param>
        public void Receive(JointMessage message)
        {
            const int PanServoIndex = 1;
            double turretPan = message.State[PanServoIndex].PositionInRadians;
            if (this.zeroPanAngle == null)
            {
                this.zeroPanAngle = turretPan;
            }

            double currentPanAngle = turretPan - this.zeroPanAngle.Value;
            
            if (double.IsNaN(this.previousPublishedAngle) || (Math.Abs(this.previousPublishedAngle - currentPanAngle) > this.turretPanThreshold))
            {
                this.originatingTime = message.OriginatingTime;
                this.previousPublishedAngle = currentPanAngle;
            }

            this.Publish<JointMessage>(this.jointPublisher, message);
        }

        /// <summary>
        /// Receive, process and republish a message
        /// </summary>
        /// <param name="message">Message to process</param>
        public void Receive(IRProximityMessage message)
        {
            this.Publish<IRProximityMessage>(this.proximityPublisher, message);
        }

        /// <summary>
        /// Receive, process and republish a message
        /// </summary>
        /// <param name="message">Message to process</param>
        public void Receive(GyroscopeMessage message)
        {
            this.Publish<GyroscopeMessage>(this.gyroscopePublisher, message);
        }

        /// <summary>
        /// Receive, process and republish a message
        /// </summary>
        /// <param name="message">Message to process</param>
        public void Receive(DifferentialDriveMessage message)
        {
            this.Publish(this.differentialDrivePublisher, message);          
        }

        /// <summary>
        /// Receive, process and republish a message
        /// </summary>
        /// <param name="message">Message to process</param>
        public void Receive(SimpleAgentMessage<TDriveFeedbackState> message)
        {            
            this.Publish<SimpleAgentMessage<TDriveFeedbackState>>(this.driveFeedbackPublisher, message);
        }

        /// <summary>
        /// Receive, process and republish a message
        /// </summary>
        /// <param name="message">Message to process</param>
        public void Receive(CompassMessage message)
        {
            this.Publish<CompassMessage>(this.compassPublisher, message);
        }

        /// <summary>
        /// Receive, process and republish a message
        /// </summary>
        /// <param name="message">Message to process</param>
        public void Receive(AccelerometerMessage message)
        {
            this.Publish<AccelerometerMessage>(this.accelerometerPublisher, message);
        }

        /// <summary>
        /// Receive, process and republish a message
        /// </summary>
        /// <param name="message">Message to process</param>
        public void Receive(BatteryMessage message)
        {
            this.Publish<BatteryMessage>(this.batteryPublisher, message);
        }

        /// <summary>
        /// Publishes the message once for each timestamp close to the timestamp of the joint message which triggered
        /// the publishing. 
        /// </summary>
        /// <typeparam name="T">The type of message</typeparam>
        /// <param name="publisher">Message publisher</param>
        /// <param name="message">Message to be published</param>
        private void Publish<T>(IForwarder<T> publisher, T message) where T : AgentMessage
        {
            bool publish = false;
            long timestampToPublish = this.originatingTime;
            const long SyncToleranceInMilliseconds = 20;

            // Always publish the game pad agent message, since that is necessary for proper metrics.
            // This will be used only when the slam recording transformer is used in direct playback.
            // An alternative would be to connect the gamepad directly bypassing the slam recorder, however  doing it this way allows us to change
            // the timestamps in the all the recorded stream is we so desire.
            // For other types of messages, publish the message once for each timestamp close to the timestamp of the joint message which triggered
            // the publishing.
            if (this.publishedTimestamps.ContainsKey(typeof(T)) && message.GetType() != typeof(GamePadAgentMessage))
            {
                if (this.publishedTimestamps[typeof(T)] != timestampToPublish
                    && Math.Abs(message.OriginatingTime - timestampToPublish) < SyncToleranceInMilliseconds)
                {
                    publish = true;
                }
            }
            else
            {
                publish = true;
            }

            if (publish)
            {
                publisher.Post(message);
                this.publishedTimestamps[typeof(T)] = timestampToPublish;
            }
        }
    }
}
